module nade.physics.joint;

import derelict.newton.newton;

import nade.vector;
import nade.matrix;

import nade.physics.rigid_body;
import nade.physics.world;


class Joint : WorldObject {
	this(World world, RigidBody child, RigidBody parent)
	{
		super(world);

		_child = child;
		_parent = parent;
	}
	~this()
	{
		if(get(_phys) !is null)
		{
			NewtonJointSetDestructor(_phys, null);
			NewtonDestroyJoint(world.phys, _phys);
		}
	}

	NewtonJoint* phys() { return _phys; }
	NewtonJoint* phys(NewtonJoint* phys)
	{
		assert(_phys is null);

		NewtonJointSetUserData(phys, cast(void*)this);
		NewtonJointSetDestructor(phys, &cbDestructor);
		return _phys = phys;
	}

	protected {
		RigidBody		_child;
		RigidBody		_parent;
	}
	private {
		NewtonJoint*	_phys = null;
	}
	static {
		extern(C) void cbDestructor(const NewtonJoint* phys)
		{
			auto joint = get(phys);

			NewtonJointSetDestructor(phys, null);
			NewtonJointSetUserData(phys, null);

			delete joint;
		}

		Joint get(const NewtonJoint* phys)
		{
			return cast(Joint)NewtonJointGetUserData(phys);
		}
	}


	static class Axis : Joint {
		this(World world, RigidBody child, RigidBody parent, const Vector3 anchor, const Vector3 direction)
		{
			super(world, child, parent);

			_anchor = anchor;
			_direction = direction;

			phys = NewtonConstraintCreateHinge(
				world.phys,
				anchor.toArray!(double).ptr,
				direction.toArray!(double).ptr,
				child.phys,
				parent.phys
				);
		}

		protected {
			Vector3		_anchor;
			Vector3		_direction;
		}
	}
}


class CustomJoint : Joint {
	this(World world, RigidBody child, RigidBody parent, int maxDOF = 6)
	{
		super(world, child, parent);

		_maxDOF = maxDOF;

		_child.addDestructorCallback(
			this,
			{
				if(_parent !is null) _parent.removeDestructorCallback(this);
				_child = null; _parent = null;
				delete this;
			}
		);
		_parent.addDestructorCallback(
			this,
			{
				if(_child !is null) _child.removeDestructorCallback(this);
				_child = null; _parent = null;
				delete this;
			}
		);

		phys = NewtonConstraintCreateUserJoint(
			world.phys,
			_maxDOF,
			&cbSubmitConstraint,
			child.phys,
			parent.phys
			);
	}

	abstract void update();

	protected {
		int				_maxDOF;

		void addLinearRow(const Vector3 point1, const Vector3 point2, const Vector3 axis)
		{
			NewtonUserJointAddLinearRow(
				_phys,
				point1.toArray!(double).ptr,
				point2.toArray!(double).ptr,
				axis.toArray!(double).ptr
				);
		}
	}

	static extern(C) void cbSubmitConstraint(const NewtonJoint* phys)
	{
		auto joint = cast(CustomJoint)Joint.get(phys);
		assert(joint !is null);
		joint.update();
	}


	static class Weld : CustomJoint {
		this(World world, RigidBody child, RigidBody parent, const Vector3 anchor)
		{
			super(world, child, parent);

			_childMatrix =
				Matrix44.translation(anchor)
				*(*child.matrix).inverse
				;
			_parentMatrix =
				Matrix44.translation(anchor)
				*(*parent.matrix).inverse
				;
		}

		override void update()
		{
			const childAnchor = _childMatrix*(*_child.matrix);
			const childAnchorX = childAnchor.origin + childAnchor.axisX*50;
			const childAnchorY = childAnchor.origin + childAnchor.axisY*50;

			const parentAnchor = _parentMatrix*(*_parent.matrix);
			const parentAnchorX = parentAnchor.origin + parentAnchor.axisX*50;
			const parentAnchorY = parentAnchor.origin + parentAnchor.axisY*50;

			addLinearRow(childAnchor.origin, parentAnchor.origin, Vector3.AXIS_X);
			addLinearRow(childAnchor.origin, parentAnchor.origin, Vector3.AXIS_Y);
			addLinearRow(childAnchor.origin, parentAnchor.origin, Vector3.AXIS_Z);
			addLinearRow(childAnchorY, parentAnchorY, childAnchor.axisZ);
			addLinearRow(childAnchorX, parentAnchorX, childAnchor.axisY);
			addLinearRow(childAnchorX, parentAnchorX, childAnchor.axisZ);
		}

		protected {
			Matrix44	_childMatrix;
			Matrix44	_parentMatrix;
		}
	}
}







